use uom::si::{
    acceleration::{foot_per_second_squared, meter_per_second_squared},
    angular_acceleration::radian_per_second_squared,
    f64::*,
    force::newton,
    mass::kilogram,
    ratio::{percent, ratio},
    torque::newton_meter,
    velocity::{foot_per_second, knot, meter_per_second},
};

use crate::{
    engine::Engine,
    shared::{interpolation, ReverserPosition},
    simulation::{SimulationElement, SimulatorWriter, UpdateContext, Write},
};

use crate::simulation::{InitContext, VariableIdentifier};

#[derive(Copy, Clone)]
struct ReverserThrust {
    current_thrust: Force,
}
impl ReverserThrust {
    const MIN_REVERSER_POS: f64 = 0.2;

    fn new() -> Self {
        Self {
            current_thrust: Force::default(),
        }
    }
    fn update(
        &mut self,
        engine: &impl Engine,
        reverser_position: &impl ReverserPosition,
        longitudinal_velocity: Velocity,
    ) {
        self.current_thrust =
            if reverser_position.reverser_position().get::<ratio>() > Self::MIN_REVERSER_POS {
                let current_thrust_force =
                    Force::new::<newton>(engine.net_thrust().get::<kilogram>() * 9.80665);

                // Total net thrust with reverser will need to cancel out forward thrust generated by engine + added desired reverse thrust
                (current_thrust_force
                    + current_thrust_force
                        * Self::reverse_thrust_ratio_from_n1(engine.corrected_n1())
                        * Self::reverse_thrust_ratio_longitudinal_velocity(longitudinal_velocity))
                    * reverser_position.reverser_position()
            } else {
                Force::default()
            };
    }

    fn current_thrust(&self) -> Force {
        self.current_thrust
    }

    fn reverse_thrust_ratio_from_n1(engine_n1: Ratio) -> Ratio {
        let n1_breakpoints = [0., 15., 20., 50., 55., 100.];

        let reverse_thrust_ratio = [0., 0., 0.04, 0.31, 0.31, 0.31];

        Ratio::new::<ratio>(interpolation(
            &n1_breakpoints,
            &reverse_thrust_ratio,
            engine_n1.get::<percent>(),
        ))
    }

    fn reverse_thrust_ratio_longitudinal_velocity(longitudinal_velocity: Velocity) -> Ratio {
        let longitudinal_velocity_knots = longitudinal_velocity.get::<knot>();

        if longitudinal_velocity_knots >= 0. {
            Ratio::new::<ratio>(1.0)
        } else {
            Ratio::new::<ratio>((0.1151 * longitudinal_velocity_knots).exp())
        }
    }
}

pub struct ReverserForce {
    reverser_delta_speed_id: VariableIdentifier,
    reverser_angular_accel_id: VariableIdentifier,
    reverser_delta_accel_id: VariableIdentifier,

    reversers: [ReverserThrust; 2],

    plane_delta_speed_due_to_reverse_thrust: Velocity,
    plane_delta_acceleration_due_to_reverse_thrust: Acceleration,

    dissimetry_acceleration: AngularAcceleration,
}
impl ReverserForce {
    const DISTANCE_FROM_CG_TO_ENGINE_METER: f64 = 5.;

    pub fn new(context: &mut InitContext) -> Self {
        Self {
            reverser_delta_speed_id: context.get_identifier("REVERSER_DELTA_SPEED".to_owned()),
            reverser_angular_accel_id: context
                .get_identifier("REVERSER_ANGULAR_ACCELERATION".to_owned()),
            reverser_delta_accel_id: context.get_identifier("REVERSER_DELTA_ACCEL".to_owned()),

            reversers: [ReverserThrust::new(); 2],
            plane_delta_speed_due_to_reverse_thrust: Velocity::default(),
            plane_delta_acceleration_due_to_reverse_thrust: Acceleration::default(),
            dissimetry_acceleration: AngularAcceleration::default(),
        }
    }

    pub fn update(
        &mut self,
        context: &UpdateContext,
        engine: [&impl Engine; 2],
        reverser_position: &[impl ReverserPosition],
    ) {
        for (engine_index, reverser) in self.reversers.iter_mut().enumerate() {
            reverser.update(
                engine[engine_index],
                &reverser_position[engine_index],
                Velocity::new::<meter_per_second>(context.local_velocity().to_ms_vector()[2]),
            );
        }

        let total_force = self.reversers[0].current_thrust() + self.reversers[1].current_thrust();

        self.plane_delta_acceleration_due_to_reverse_thrust =
            if context.total_weight().get::<kilogram>() > 0. {
                -total_force / context.total_weight()
            } else {
                Acceleration::default()
            };

        self.plane_delta_speed_due_to_reverse_thrust = Velocity::new::<meter_per_second>(
            self.plane_delta_acceleration_due_to_reverse_thrust
                .get::<meter_per_second_squared>()
                * context.delta_as_secs_f64(),
        );

        let total_dissimetry =
            self.reversers[1].current_thrust() - self.reversers[0].current_thrust();

        let dissimetry_torque = Torque::new::<newton_meter>(
            total_dissimetry.get::<newton>() * Self::DISTANCE_FROM_CG_TO_ENGINE_METER,
        );

        self.dissimetry_acceleration = if context.total_yaw_inertia_kg_m2().abs() > 0. {
            AngularAcceleration::new::<radian_per_second_squared>(
                dissimetry_torque.get::<newton_meter>() / context.total_yaw_inertia_kg_m2(),
            )
        } else {
            AngularAcceleration::default()
        };
    }
}
impl SimulationElement for ReverserForce {
    fn write(&self, writer: &mut SimulatorWriter) {
        writer.write(
            &self.reverser_delta_speed_id,
            self.plane_delta_speed_due_to_reverse_thrust
                .get::<foot_per_second>(),
        );

        writer.write(
            &self.reverser_angular_accel_id,
            self.dissimetry_acceleration
                .get::<radian_per_second_squared>(),
        );

        writer.write(
            &self.reverser_delta_accel_id,
            self.plane_delta_acceleration_due_to_reverse_thrust
                .get::<foot_per_second_squared>(),
        );
    }
}
